/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include <Arduino.h>
#include "BluetoothReceiver.h"

void BluetoothReceiver::setup()
{

}

void BluetoothReceiver::receive()
{
   uint8_t action=0;

  while (bleSerial_.available() > 0) {
    uint8_t in = bleSerial_.read();
    delay(2);
    if (in == -1)
      continue;

    Serial.print("input:");
    Serial.println(in, BIN);
    action = in;
  }

  if (action > 64) {
     return;
  }
     
  backwardPressState_ = (action & 0b0000001);
  forwardPressState_ =  (action & 0b0000010) >> 1;
  leftPressState_ =     (action & 0b0000100) >> 2;
  rightPressState_ =    (action & 0b0001000) >> 3;
  accelPressState_ =    (action & 0b0010000) >> 4;
  stopPressState_ =     (action & 0b0100000) >> 5;

  if (backwardPressState_ ==1) {
    angleValue_ =0;
  } else if (forwardPressState_ ==1) {
    angleValue_ =0;
  } else if (leftPressState_ ==1) {
    if (angleValue_ >-20)
      angleValue_ -=6;
    if (angleValue_ >-35)
      angleValue_ -=4;
    else
      angleValue_ -=2;
    
    if (angleValue_ <-45)
      angleValue_ =-45;
  } else if (rightPressState_ ==-1) {
    if (angleValue_ <20)
      angleValue_ +=6;
    if (angleValue_ <35)
      angleValue_ +=4;
    else
      angleValue_ +=2;
      
    if (angleValue_ >45)
      angleValue_ =45;
  }
  
  if (accelPressState_ ==1) {
     if (accelValue_ < 100)
        accelValue_ +=1;
  } else {
    if (accelValue_ >0) {
      accelValue_ -=1;
    }
  }
  if (stopPressState_ ==1) {
     accelValue_ =0;
     angleValue_ =0;
  }

  // feedback
  bleSerial_.write("car accel:");
  bleSerial_.write(accelValue_);
  bleSerial_.println();
}

BluetoothReceiver::~BluetoothReceiver()
{
}
